using System.Collections.Generic;
using Microsoft.Xna.Framework;
using System.Linq;
using System.Diagnostics;
using GameEngine.Units;
using GameEngine.Collections.Generic;

namespace GameEngine.Navigation
{
	public class NavigationMap
	{
		private WayPoint TheGrandFather;
		public List<WayPoint> GroupWayPoints;
		public List<WayPoint> WayPoints;

		public NavigationMap(NavigationMesh mesh)
		{
			this.Mesh = mesh;

			// generate high resolution waypoint map
			this.WayPoints = new List<WayPoint>(this.Vertices.Length);
			for (int i = 0; i < this.Vertices.Length; i++)
			{
				this.WayPoints.Add(new WayPoint() { Position = this.Vertices[i] });
			}
			Dictionary<Edge, bool> edges = new Dictionary<Edge, bool>();
			for (int i = 0; i < this.Faces.Length; i++)
			{
				Face face = this.Faces[i];
				edges[new Edge(face.A, face.B)] = true;
				edges[new Edge(face.B, face.C)] = true;
				edges[new Edge(face.C, face.A)] = true;
			}
			foreach (var edge in edges.Keys)
			{
				this.WayPoints[edge.A].Neighbours.Add(this.WayPoints[edge.B]);
				this.WayPoints[edge.B].Neighbours.Add(this.WayPoints[edge.A]);
			}

			// Allow overjump on the high resolution map through 1 neighbour so better angle density could be achieved
			foreach (WayPoint sourceWayPoint in this.WayPoints)
			{
				Dictionary<WayPoint, int> neighbours = new Dictionary<WayPoint, int>();
				foreach (WayPoint neighbourWayPoint in sourceWayPoint.Neighbours)
				{
					foreach (WayPoint secondNeighbour in neighbourWayPoint.Neighbours)
					{
						if (!neighbours.ContainsKey(secondNeighbour))
						{
							neighbours[secondNeighbour] = 1;
						}
						else
						{
							neighbours[secondNeighbour] += 1;
						}
					}
				}
				foreach (WayPoint neighbour in neighbours.Keys)
				{
					if (neighbour != sourceWayPoint && !sourceWayPoint.Neighbours.Contains(neighbour))
					{
						sourceWayPoint.Neighbours.Add(neighbour);
					}
				}
			}

			// use the smoothing groups to generate low level connectivity graph
			this.GroupWayPoints = new List<WayPoint>();
			Dictionary<FaceGroup, IList<WayPoint>> groups = new Dictionary<FaceGroup, IList<WayPoint>>();
			foreach (Face face in this.Faces)
			{
				FaceGroup faceGroup = new FaceGroup(face.Groups);
				if (!groups.ContainsKey(faceGroup))
				{
					groups.Add(faceGroup, new List<WayPoint>());
				}
				IList<WayPoint> children = groups[faceGroup];
				children.Add(this.WayPoints[face.A]);
				children.Add(this.WayPoints[face.B]);
				children.Add(this.WayPoints[face.C]);
			}
			////foreach (KeyValuePair<FaceGroup, IList<WayPoint>> pair in groups)
			////{
			////    Debug.WriteLine(pair.Key);
			////}
			Dictionary<FaceGroup, WayPoint> groupWayPoints = new Dictionary<FaceGroup, WayPoint>();
			foreach (KeyValuePair<FaceGroup, IList<WayPoint>> pair in groups)
			{
				Vector3 center = new Vector3(0, 0, 0);
				WayPoint parentWayPoint = new WayPoint();
				this.GroupWayPoints.Add(parentWayPoint);
				foreach(WayPoint wayPoint in pair.Value)
				{
					center += wayPoint.Position / pair.Value.Count;
					wayPoint.Parent = parentWayPoint;
				}
				parentWayPoint.Position = center;
				groupWayPoints[pair.Key] = parentWayPoint;
			}
			foreach (KeyValuePair<FaceGroup, WayPoint> pair in groupWayPoints)
			{
				foreach (KeyValuePair<FaceGroup, WayPoint> neighbourPair in groupWayPoints)
				{
					if (pair.Value != neighbourPair.Value)
					{
						bool neighbourHasAllParentKeys = true;
						foreach (int i in pair.Key.groups)
						{
							if (!neighbourPair.Key.groups.Contains(i))
							{
								neighbourHasAllParentKeys = false;
								break;
							}
						}
						if (neighbourHasAllParentKeys)
						{
							////Debug.WriteLine("Link: {0}, {1}", pair.Key, neighbourPair.Key);
							pair.Value.Neighbours.Add(neighbourPair.Value);
							neighbourPair.Value.Neighbours.Add(pair.Value);
						}
					}
				}
			}

			// This is used to determine the bottom of the resolution stack
			this.TheGrandFather = new WayPoint();
			foreach (WayPoint wayPoint in this.GroupWayPoints)
			{
				wayPoint.Parent = this.TheGrandFather;
			}
		}

		public NavigationMesh Mesh { get; private set; }
		public Vector3[] Vertices
		{
			get
			{
				return this.Mesh.Vertices;
			}
		}
		public Face[] Faces
		{
			get
			{
				return this.Mesh.Faces;
			}
		}

		private static long searchCount = 0;

		public static IList<WayPoint> FindPath(WayPoint start, WayPoint goal, float distance, float radius, IEnumerable<IPathBlocker> blockers)
		{
			WayPoint parentGoal = null;
			if (start.Parent != goal.Parent && !start.Parent.Neighbours.Contains(goal.Parent))
			{
				// the two points are in more than two jumps over the low resolution graph
				IList<WayPoint> lowResPath = NavigationMap.FindPath(start.Parent, goal.Parent, 0, 0, null);
				parentGoal = lowResPath[1];
			}

			// Evil devices of memory consumption... move them in the NavigationMap so containers may be recycled... too bad we need to support multy level A*... these will mess for the low and high res maps
			IDictionary<WayPoint, bool> closedSet = new Dictionary<WayPoint, bool>();
			BinaryHeap<WayPoint> openHeap = new BinaryHeap<WayPoint>(new WayPointCostComparer());
			IDictionary<WayPoint, bool> openSet = new Dictionary<WayPoint, bool>();

			openSet[start] = true;
			openHeap.Insert(start);
			start.ReachedFrom = null;
			start.CurrentCost = 0;
			start.HeuristicCost = Vector3.Distance(start.Position, goal.Position);
			start.TotalCost = start.HeuristicCost;
			WayPoint x;
			while (openHeap.Remove(out x))
			{
				if (x == goal || x.HeuristicCost < distance || x.Parent == parentGoal)
				{
					List<WayPoint> result = new List<WayPoint>();
					while (x != null)
					{
						result.Add(x);
						x = x.ReachedFrom;
					}
					result.Reverse();
					Debug.WriteLine("{0}: ClosedSet: {1}, OpenSet: {2}", NavigationMap.searchCount++, closedSet.Count, openSet.Count);
					return result;
				}
				closedSet[x] = true;
				foreach (WayPoint y in x.Neighbours)
				{
					if (closedSet.ContainsKey(y))
					{
						continue;
					}
					if (y.Parent != start.Parent && !start.Parent.Neighbours.Contains(y.Parent))
					{
						closedSet.Add(y, true);
						continue; // too far from start. we do not want to open the entire hi-res map
					}
					if (blockers != null)
					{
						// Could be 3 lines of code using Linq but it is totatly undebugable...
						bool blocked = false;
						foreach (IPathBlocker blocker in blockers)
						{
							float blockDistance = Vector3.Distance(blocker.Position, y.Position);
							if (blockDistance < radius + blocker.Radius)
							{
								blocked = true;
								break;
							}
						}
						if (blocked)
						{
							closedSet.Add(y, true);
							continue;
						}
					}
					float CostFromX = x.CurrentCost + Vector3.Distance(x.Position, y.Position);
					bool isYOpen = openSet.ContainsKey(y);
					if (isYOpen && CostFromX < y.CurrentCost)
					{
						// allready opened but a better source is found
						y.ReachedFrom = x;
						y.CurrentCost = CostFromX;
						y.TotalCost = y.CurrentCost + y.HeuristicCost * 1.3f;
					}
					if (!isYOpen)
					{
						// opening non-opened
						y.ReachedFrom = x;
						y.CurrentCost = CostFromX;
						y.HeuristicCost = Vector3.Distance(y.Position, goal.Position);
						y.TotalCost = y.CurrentCost + y.HeuristicCost * 1.3f;
						openSet[y] = true;
						openHeap.Insert(y);
					}
				}
			}
			// In case no path is found return path to the closest possible location...
			Debug.WriteLine("{0}: ClosedSet: {1}, OpenSet: {2}, WARNING! NOT FOUND!", NavigationMap.searchCount++, closedSet.Count, openSet.Count);
			return new List<WayPoint>();
		}
	}
}
